//
// Created by augtons on 2022/1/20.
//

#include "moto_ctrl.h"

int moto_array[4][2] = {
    {MCPWM_UNIT_0, MCPWM_TIMER_0},
    {MCPWM_UNIT_0, MCPWM_TIMER_1},
    {MCPWM_UNIT_0, MCPWM_TIMER_2},
    {MCPWM_UNIT_1, MCPWM_TIMER_0},
};

void moto_forward(int motoIndex, float duty){
    if(duty == 0){
        moto_stop(motoIndex);
        return;
    }
    if(duty > 100){
        duty = 100;
    }else if(duty < 0){
        duty = 0;
    }
    mcpwm_set_signal_low(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_A);
    mcpwm_set_duty(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_B, duty);
    mcpwm_set_duty_type(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_B, MCPWM_DUTY_MODE_0);
}

void moto_backward(int motoIndex, float duty){
    if(duty == 0){
        moto_stop(motoIndex);
        return;
    }
    if(duty > 100){
        duty = 100;
    }else if(duty < 0){
        duty = 0;
    }
    mcpwm_set_signal_low(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_B);
    mcpwm_set_duty(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_A, duty);
    mcpwm_set_duty_type(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_A, MCPWM_DUTY_MODE_0);
}

void moto_stop(int motoIndex){
    mcpwm_set_signal_low(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_A);
    mcpwm_set_signal_low(moto_array[motoIndex][0], moto_array[motoIndex][1], MCPWM_GEN_B);
}

void init_mcp(){
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, MOTO1A);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, MOTO1B);

    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, MOTO2A);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, MOTO2B);

    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, MOTO3A);
    mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, MOTO3B);

    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM0A, MOTO4A);
    mcpwm_gpio_init(MCPWM_UNIT_1, MCPWM0B, MOTO4B);

    mcpwm_config_t mcpwmConfig = {
        .frequency = 200,
        .cmpr_a = 0,
        .cmpr_b = 0,
        .counter_mode = MCPWM_UP_COUNTER,
        .duty_mode = MCPWM_DUTY_MODE_0,
    };

    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &mcpwmConfig);
    mcpwm_init(MCPWM_UNIT_1, MCPWM_TIMER_0, &mcpwmConfig);

    moto_stop(0);
    moto_stop(1);
    moto_stop(2);
    moto_stop(3);
}